/*
* Ljudföjlare v1.0
*
* Grupp 15
* "Om en framtida grupp funderar på att göra ett liknande projekt - don't! Mikrofonerna är alldeles för imprecisa för att fungera på ett rimligt avstånd"
*/
#define F_CPU 1600000UL

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>


#define ADC_value ADC

//Variabler
volatile int threshold = 25;
volatile uint16_t button_state = 0;
volatile int mic1 = 0;
volatile int mic2 = 0;
volatile int mic3 = 0;
volatile int mic1_old = 0;
volatile int mic2_old = 0;
volatile int mic3_old = 0;


void setup(){
	ADCSRA = (1<<ADEN)|(1<<ADPS0)|(1<<ADPS1)|(1<<ADPS2);//ADC ON och Division Factor
	PRR0 = (0<<PRADC);//Sätt PRADC till 1 om ADC ska stängas av
	DDRD = 0xFF;//Alla D-pins är outputs
	DDRB |= 0xFF;//Alla B-pins är outputs
	DDRA = 0b00;//Alla A-pins är inputs
}

int read_button() { //Läser av knappen
	const uint8_t debounce_time = 50;

	if (PINA & (1 << PINA3)) {
	_delay_ms(debounce_time);
		if (PINA & (1 << PINA3)) {
			return 1;
		}
	}
	return 0;
}

void OC0ACounter(int speed){ //Justerar hastigheten på PWM
	DDRB |= (1 << PORTB3);
	OCR0A = speed;
	TCCR0A |= (1<<COM0A1)|(1<<WGM02)|(1<<WGM01) | (1<<WGM00);
	TCCR0B |= (1<<CS00);
}

void OC0BCounter(int speed){ //Justerar hastigheten på PWM
	DDRB |= (1<<PORTB4);
	OCR0B = speed;
	TCCR0A |= (1<<COM0B1) | (1<<COM0B0)|(1<<WGM02)|(1<<WGM01) | (1<<WGM00);
	TCCR0B |= (1<<CS00);
}

void goForward(void){
	OC0BCounter(50); //vänster (~50)
	OC0ACounter(210); //höger (~200)
}

void goLeft(void){
	OC0ACounter(210); //höger (~200)
}
void goRight(void){
	OC0BCounter(50); //vänster (~50)
}

void stop(void){//Avbryter PWM
	DDRB = 0x00;//Sätter alla B-pins till inputs
}

void toggleLedOn1(){//Sätter på gula led
	PORTD = (1<<PORTD2);
}

void toggleLedOff1(){//Stänger av gula led
	PORTD = (0<<PORTD2);
}

void init_adc_1(){//Låter ADC:n läsa från channel 1 med single ended input
	ADMUX = (0<<MUX4)|(0<<MUX3)|(0<<MUX2)|(0<<MUX1)|(0<<MUX0)|(0<<ADLAR)|(0<<REFS1)|(0<<REFS0); //Input channel 1
}

void init_adc_2(){//Låter ADC:n läsa från pin 2 med single ended input
	ADMUX = (0<<MUX4)|(0<<MUX3)|(0<<MUX2)|(0<<MUX1)|(1<<MUX0)|(0<<ADLAR)|(0<<REFS1)|(0<<REFS0); //Input channel 2
}

void init_adc_3(){//Låter ADC:n läsa från pin 3 med single ended input
	ADMUX = (0<<MUX4)|(0<<MUX3)|(0<<MUX2)|(1<<MUX1)|(0<<MUX0)|(0<<ADLAR)|(0<<REFS1)|(0<<REFS0); //Input channel 3
}

void init_conversion(){//Startar ADC-conversion
	ADCSRA |= (1<<ADSC);
}

void wait_for_conversion(){//Väntar på att conversionen blir klar
	while (ADCSRA&(1<<ADSC)){};
}

void read_mic1(){//Läser av mikrofon 1
	init_adc_1();
	mic1 = 0;
for(int i = 0; i<15; i++){
	
		int mic;
		init_conversion();
	wait_for_conversion();
		mic = ADC_value;
	_delay_us(100);
	 mic1 = mic1 + mic;
		
	}
	mic1 = round((mic1/15)*0.98);
	mic1_old = mic1;
}


void read_mic2(){//Läser av mikrofon 2
		init_adc_2();
		mic2 = 0;
for(int i = 0; i<15; i++){
	
		int mic;
		init_conversion();
	wait_for_conversion();
		mic = ADC_value;
	_delay_us(100);
	 mic2 = mic2 + mic;
		
	}
	mic2 = round((mic2/15)*0.97);
	mic2_old = mic2;
}

void read_mic3(){//Läser av mikrofon 3
		init_adc_3();
		mic3 = 0;
for(int i = 0; i<15; i++){
	
		int mic;
		init_conversion();
	wait_for_conversion();
		mic = ADC_value;
	_delay_us(100);
	 mic3 = mic3 + mic;
		
	}
	mic3 = round(mic3/15);
	mic3_old = mic3;
}

void movement(){//Själva koden för att få roboten att röra på sig
	read_mic1();
	
	read_mic2();
	
	read_mic3();
	
	if((mic1 > 170) || (mic2 > 170) ||(mic3 > 170)){
		
		if((mic3 - threshold) > mic2 && (mic3 - threshold) > mic1){
			goForward();
			_delay_ms(500);
			stop();
			_delay_ms(500);
		}
		else if((mic1 - threshold) > mic2 && (mic1 - threshold) > mic3){
			goRight();
			_delay_ms(260);
			stop();
			_delay_ms(500);
		}
		else if((mic2 - threshold) > mic3 && (mic2 - threshold) > mic1){
			goLeft();
			_delay_ms(260);
			stop();
			_delay_ms(500);
		}
	}
}


int main(void)
{
	_delay_ms(1000);
	stop();
	setup();

	

	while(1){
		
		if(read_button() == 1){//Gör så att roboten kan togglas av och på med hjälp av knappen
			while(read_button() == 1){
				toggleLedOn1();
				movement();}
				button_state = 1;
				toggleLedOff1();
				while(button_state == 1){
					if(read_button() == 1){
						while(read_button() == 1){}
						button_state = 0;
					}
				}
		}

	}							
}